from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare

def generate_launch_description():
    # 设置TurtleBot4型号
    model = 'Standard'
    
    return LaunchDescription([
        # 设置环境变量
        SetEnvironmentVariable(name='TURTLEBOT3_MODEL', value=model),
        
        # 启动Gazebo仿真
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource([
                PathJoinSubstitution([
                    FindPackageShare('turtlebot4_gazebo'),
                    'launch',
                    'turtlebot4_world.launch.py'
                ])
            ]),
            launch_arguments={'model': model}.items()
        ),
        
        # 启动RViz
        Node(
            package='rviz2',
            executable='rviz2',
            name='rviz2',
            arguments=['-d', PathJoinSubstitution([
                FindPackageShare('turtlebot4_cartographer'),
                'rviz',
                'turtlebot4_cartographer.rviz'
            ])],
            output='screen'
        ),
        
        # 启动控制器节点
        Node(
            package='closed_control_py',  # 替换为你的包名
            executable='closed_control_node',
            output='screen'
        )
    ])